#include <Windows.h>
#include <string>
#include <iostream>

#include "BrainException.h"
#include "BTChannel.h"


CBTChannel::CBTChannel(void)
{
	bConnected = false;
}


CBTChannel::~CBTChannel(void)
{
	disconnect();
}

void CBTChannel::connect(void)
{
//  Open a handle to the specified com port.
     hCom = CreateFile( "COM7",
                        GENERIC_READ | GENERIC_WRITE,
                        0,      //  must be opened with exclusive-access
                        NULL,   //  default security attributes
                        OPEN_EXISTING, //  must use OPEN_EXISTING
                        0,      //  not overlapped I/O
                        NULL ); //  hTemplate must be NULL for comm devices
  
     if (hCom == INVALID_HANDLE_VALUE) 
     {
		 throw BrainException("Cannot connect to COM port");
     }
	 DCB dcb;
	 BOOL fSuccess;
     //  Initialize the DCB structure.
     SecureZeroMemory(&dcb, sizeof(DCB));
     dcb.DCBlength = sizeof(DCB);
  
     //  Build on the current configuration by first retrieving all current
     //  settings.
     fSuccess = GetCommState(hCom, &dcb);
  
     if (!fSuccess) 
     {
        //  Handle the error.
		 throw BrainException("Cannot get COM state");
     }
  
     //  Fill in some DCB values and set the com state: 
     //  57,600 bps, 8 data bits, no parity, and 1 stop bit.
     dcb.BaudRate = CBR_115200;     //  baud rate
     dcb.ByteSize = 8;             //  data size, xmit and rcv
     dcb.Parity   = NOPARITY;      //  parity bit
     dcb.StopBits = ONESTOPBIT;    //  stop bit
  
     fSuccess = SetCommState(hCom, &dcb);
  
     if (!fSuccess) 
     {
        //  Handle the error.
		 throw BrainException("Cannot set COM state");
     }
}

void CBTChannel::disconnect(void)
{
	if (bConnected) {
		CloseHandle(hCom);
	}
}

int CBTChannel::getDistance(void)
{
	sendCommand("q");
	std::string s = readAnswer();
	//std::cout << "Read string: " << s << std::endl;
	char str1 [100];
	char str2 [100];
	int d;
	sscanf(s.c_str(), "%s %s %d", str1, str2, &d);
	//std::cout << "After scan: s1 = '" << str1 << "', s2='" << str2 << "', d=" << d << std::endl;
	return d;
}

void CBTChannel::setEyeAngle(int angle)
{
	if (angle <= 90 && angle >= 0) {
		char s [10];
		sprintf(s, "v%02d\r\n", 90-angle);
		std::cout << "Sending: " << s << std::endl;
		sendCommand(std::string(s));
	}
	if (angle < 0 && angle >= -90) {
		char s [10];
		sprintf(s, "u%02d\r\n", 90+angle);
		std::cout << "Sending: " << s << std::endl;
		sendCommand(std::string(s));
	}
}

void CBTChannel::forward(void)
{
	sendCommand("f");
}

void CBTChannel::left(void)
{
	sendCommand("l");
}

void CBTChannel::right(void)
{
	sendCommand("r");
}

void CBTChannel::back(void)
{
	sendCommand("b");
}

void CBTChannel::stop(void)
{
	sendCommand("s");
}

void CBTChannel::sendCommand(std::string command)
{
	BOOL fSuccess;
	DWORD iWritten;
	fSuccess = ::WriteFile(hCom, command.c_str(), command.length(), &iWritten, NULL);
	if (!fSuccess || iWritten != command.length()) {
		 throw BrainException("Cannot send command");
	}
}

std::string CBTChannel::readAnswer()
{
	BOOL fSuccess;
	DWORD iRead;
	const int MAX_READ = 100;
	const char LF = 10;
	char buf [MAX_READ];
	int pos = 0;
	char ch;
	ch = ' ';
	while ((pos < MAX_READ) && (ch != LF)) {
		fSuccess = ::ReadFile(hCom, &ch, 1, &iRead, NULL);
		if (!fSuccess || iRead != 1) {
			 throw BrainException("Cannot read command");
		}
		buf [pos] = ch;
		++pos;
	}
	buf [pos] = '\0';
	return std::string(buf);
}


